﻿using System;
using System.Collections.Generic;
using UnityEngine;
using Windows.Kinect;



/// <summary>
/// Only Detect right hand raise gesture
/// </summary>
public class GestureRaise : GestureBase
{
    public static readonly float SKIP_TIME = 0.5f;
    public float StepFactor = 20f;
    protected enum DetectStep
    {
        DetectFailed,
        DetectStart,
        DetectProgress,
        DetectFinished,
    }

    protected Body detectBody = null;
    protected DetectStep step = DetectStep.DetectFailed;
    protected Vector3 lastRightPos = Vector3.zero;
    protected Vector3 startPos = Vector3.zero;
    protected float curTime = 0;
    protected float maxRaiseDistance = 0;
    private MovingAverage SmoothValuer = new MovingAverage(20);


    public override void OnTrace(Windows.Kinect.Body body)
    {
        if (body == null)
            return;
        Vector3 spineMid = GestureHelper.GetVector3FromJoint(body.Joints[JointType.SpineMid]);
        Vector3 handRight = GestureHelper.GetVector3FromJoint(body.Joints[JointType.HandRight]);
        
        if (step == DetectStep.DetectFailed && spineMid.y > handRight.y)
        {
            SetMaxRaiseDistance(body);
            startPos = lastRightPos = handRight;
            curTime = 0f;
            fValue = fVelocity = 0f;
            step = DetectStep.DetectStart;
            if (Handler!=null)
                Handler.OnDetectStart();
            debugCurve = new AnimationCurve();
        }
        else if(step == DetectStep.DetectStart)
        {
            lastRightPos = handRight;
            fValue = fVelocity = 0f;
            curTime = 0f;
            step = DetectStep.DetectProgress;
            if (Handler != null)
                Handler.OnDetectProgress(fValue);
            debugCurve.AddKey(curTime, fValue);
        }
        else if(step == DetectStep.DetectProgress)
        {
            fVelocity = (lastRightPos.y - handRight.y) / Time.deltaTime;
            curTime += Time.deltaTime;
            if (curTime < SKIP_TIME)
                return;
            fValue = SmoothValuer.SmoothValue((handRight.y - startPos.y) / maxRaiseDistance);
            //Mathf.Lerp(0, (handRight.y - startPos.y) / maxRaiseDistance, Time.deltaTime * StepFactor);
            
            lastRightPos = handRight;
            if (Handler != null)
                Handler.OnDetectProgress(fValue);
            if(fValue > 1)
                step = DetectStep.DetectFinished;
            debugCurve.AddKey(curTime, fValue);
        }
        else if(step == DetectStep.DetectFinished)
        {
            fValue = 1;
            if (Handler != null)
                Handler.OnDetectFinish();
            step = DetectStep.DetectFailed;
            //debugCurve = null;
        }

    }

    public override void OnNoTrace(Windows.Kinect.Body body)
    {
        fValue = 0;
        fVelocity = 0;
        step = DetectStep.DetectFailed;
    }

    public override void Destroy()
    {

    }

    public override bool IsInGesture(Windows.Kinect.Body body)
    {
        Vector3 handRight = GestureHelper.GetVector3FromJoint(body.Joints[JointType.HandRight]);
        Vector3 spineBase = GestureHelper.GetVector3FromJoint(body.Joints[JointType.SpineBase]);
        Vector3 spineMid = GestureHelper.GetVector3FromJoint(body.Joints[JointType.SpineMid]);
        if (step == DetectStep.DetectFailed)
        {
            return handRight.y > (spineBase.y + spineMid.y) / 2;
        }
        else
        {
            return handRight.y > spineBase.y;
        }
    }

    public override float GetValue(Windows.Kinect.Body body)
    {
        switch(step)
        {
            case DetectStep.DetectFailed:
                return 0;
            case DetectStep.DetectProgress:
                return fValue;
            case DetectStep.DetectStart:
                return 0f;
            case DetectStep.DetectFinished:
                return 1;
        }
        return 0;
    }

    protected float GetIdealValue(float curValue)
    {
        return 0f;
    }

    protected float GetIdealVelocity(float velocity)
    {
        return Mathf.Clamp(velocity, fMinValue, fMaxVelocity);
    }

    protected void SetMaxRaiseDistance(Windows.Kinect.Body body)
    {
        Vector3 spineBase = GestureHelper.GetVector3FromJoint(body.Joints[JointType.SpineBase]);
        Vector3 spineShoulder = GestureHelper.GetVector3FromJoint(body.Joints[JointType.SpineShoulder]);
        maxRaiseDistance = (spineShoulder.y - spineBase.y)*1.3f;
    }
}

